/**
  ******************************************************************************
  * File Name          : huskylens.cpp
  * Description       : huskylens program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <huskylens_ros/huskylens.h>

std::string _port;
serial::Serial ser;



sensor_msgs::CameraInfo info;
ros::Publisher pub_info;
ros::Subscriber sub_mode;
ros::Subscriber sub_enb;


int idx = 0;
int mode = 0;

unsigned char txd[TXD_MAX]; 
unsigned char rxd[RXD_MAX];

std::string id_s[ID_MAX] = {"0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10"};
                    
/* Private function prototypes -----------------------------------------------*/

/**
  * @brief
  * @param
  * @retval
  */
int huskylens_merge(unsigned char dt1, unsigned char dt2)
{
    int rst = dt2;
    rst <<= 8;
    return (rst+dt1);
}

/**
  * @brief
  * @param
  * @retval
  */
unsigned char huskylens_parse(unsigned char buf[RXD_MAX], unsigned char index)
{
    int i = 0;
    int rst = 0;
    unsigned char id = 0;
    unsigned int num = 0;
    unsigned int type = 0;
    unsigned char data[DATA_MAX];
#if 0
    printf("RXD:[");
    for(i=0; i<index; i++)
    {
        printf(" %02X ", rxd[i]);
    }
    printf("]\r\n");
#endif 
    type = index%16;
    switch(type)
    {
        case 0:
            num = index/16;
            for(i=0; i<num; i++)
            {
                if((rxd[i*16+0]==0x55)&&(rxd[i*16+1]==0xAA)&&(rxd[i*16+2]==0X11))
                {
                    switch(rxd[i*16+4]) 
                    {
                        case 0x29:  //解析信息
                            //暂不处理
                            printf("暂不处理当前命令\r\n");
                            break;
                        case 0x2A:  //解析方框
                        case 0x2B:  //解析箭头
                            memcpy(data, &rxd[i*16+5], DATA_MAX);
                            id = huskylens_merge(data[8],data[9]);
#if 0
                            printf("DTX(%04d)->[",id);
                            for(i=0; i<DATA_MAX; i++)
                            {
                                printf(" %02X ", data[i]);
                            }
                            printf("]\r\n");
#endif
//==========================todo======================================
                            info.header.frame_id = id_s[id];
                            info.height = 320;
                            info.width = 240;
                            info.roi.x_offset = huskylens_merge(data[0],data[1]);
                            info.roi.y_offset = huskylens_merge(data[2],data[3]);
                            info.roi.height = huskylens_merge(data[4],data[5]); 
                            info.roi.width = huskylens_merge(data[6],data[7]);
                            pub_info.publish(info);
//==========================end=======================================
                            rst = 1;
                            break;
                        default:
                            printf("不支持的命令\r\n");
                            break;  
                    }
                }
            }
            break;
        case 6:
            if((rxd[0]==0x55)&&
                (rxd[1]==0xAA)&&
                (rxd[2]==0x11)&&
                (rxd[3]==0x00)&&
                (rxd[4]==0x2E)&&
                (rxd[5]==0x3E))
            {
                    printf("设置成功\r\n");   
            } 
            else
            {
                printf("设置失败\r\n");
            }
            break;
        default:
            printf("数据解析失败\r\n");
            break;
    } 
    return rst;    
}

/**
  * @brief  通信数据校验和
  * @param
  * @retval
  */
unsigned char checksum(unsigned char buf[TXD_MAX], unsigned char len)
{
    unsigned int cs = 0;
    if(len>TXD_MAX)
    {
        len = TXD_MAX;
    }
    for(int i = 0; i<len; i++)
    {
        cs+=buf[i];
    }
    return (unsigned char)cs;
}

/**
  * @brief
  * @param
  * @retval
  */
int tx_sig(unsigned char cmd, unsigned char dt[DATA_MAX], unsigned char len)
{
    int i = 0;
    txd[0] = 0x55; 
    txd[1] = 0xAA;
    txd[2] = 0x11;
    txd[3] = len;
    txd[4] = cmd;
    for(i=0; i<len; i++)
    {
        txd[5+i] = dt[i];       
    }
    txd[len+5] = checksum(txd,len+5);
    ser.write(txd,TXD_MAX);
#if 0
    printf("TXD:[");
    for(i=0; i<(len+6); i++)
    {
        printf(" %02X ", txd[i]);
    }
    printf("]\r\n");
#endif
    return 0;
}

/**
  * @brief
  * @param
  * @retval
  */
int rx_sig(void)
{
    int rst = 0;
    int index = 0;
    index = ser.available();
    if (index)
    {
        if(index>RXD_MAX)
        {
           index = RXD_MAX; 
        }       
        ser.read(rxd, index);
        rst = huskylens_parse(rxd, index);
        index = 0;
    }
    return rst;
}

/**
  * @brief
  * @param
  * @retval
  */
void modeCB(const std_msgs::UInt8::ConstPtr & msg)
{
    unsigned char data[DATA_MAX];
    mode = msg->data;
    switch(mode)
    {
        case 0:
            printf("设置模式->人脸识别\r\n");
            break;
        case 1:
            printf("设置模式->物体追踪\r\n");
            break;
        case 2:
            printf("设置模式->物体识别\r\n");
            break;
        case 3:
            printf("设置模式->巡线跟踪\r\n");
            break;
        case 4:
            printf("设置模式->颜色识别\r\n");
            break;
        case 5:
            printf("设置模式->标签识别\r\n");
            break;
        case 6:
            printf("设置模式->物体分类\r\n");
            break;
        default:
            mode = 0;
            printf("设置模式->错误，切换到人脸识别\r\n");
            break;
    }
    data[0] = mode;
    data[1] = 0;
    tx_sig(0x2d,data,2);
}

void enableCB(const std_msgs::UInt8::ConstPtr & msg)
{
    unsigned char data[DATA_MAX];
    
    if(msg->data)
    {
        switch(mode)
        {
            //获取学习后的方框
            case 0: //人脸识别
            case 1: //物体追踪
            case 2: //物体识别
            case 4: //颜色识别
            case 5: //标签识别
            case 6: //物体分类 
                tx_sig(0x24,data,0);
                break;
            //获取学习后的箭头
            case 3: 
                tx_sig(0x25,data,0);
                break;
            default:
                printf("输入错误\r\n");
                break;
        }
    }
}

/**
  * @brief  驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
    ros::init(argc, argv, "huskylens_AI");
    ros::NodeHandle nh;
    ros::Rate loop_rate(50);

    nh.getParam("/huskylens_ros/port", _port);

    pub_info = nh.advertise<sensor_msgs::CameraInfo>("/huskylens/targets", 15);
    sub_mode = nh.subscribe("/huskylens/mode", 1, modeCB);
    sub_enb = nh.subscribe("/huskylens/enable", 1, enableCB);

    try 
    { 
    //设置串口属性，并打开串口 
        ser.setPort(_port.c_str()); 
        ser.setBaudrate(9600); 
        serial::Timeout to = serial::Timeout::simpleTimeout(5); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 

    while (ros::ok())
    {
        rx_sig();
        ros::spinOnce();
        loop_rate.sleep();
    }

    if(ser.isOpen())
    {
        ser.close();
    }
  
    return 0;
}
